""" Geometry planes in three dimensions. 
"""
import numpy as np
from operator import itemgetter
from utils.math import rotation_matrix, vec_len, area_triangle

#=== helper functions
def _det3x3(a) :
    """ determinant of a 3x3 matrix a.
    This can be also done using a numpy function numpy.linalg.det()."""
    return a[0][0] * (a[1][1]*a[2][2] - a[2][1]*a[1][2]) - a[0][1] * (
        a[1][0]*a[2][2] - a[2][0]*a[1][2]) + a[0][2] * (
        a[1][0]*a[2][1] - a[2][0]*a[1][1]) 


class Plane3D:
    """ Geometric plane represented by ax + by + cz + d = 0."""
    def __init__(self, coefficients):
        """ defines an equation of plane.
        
        coefficients is an array of [a, b, c, d] for the geometry equation
        ax + by + cz + d = 0
        Constraint: a, b, c cannot all be zero at the same time, 
            otherwise a ValueError is raised.
        Cartesian coordinate system.
        """
        a, b, c, d = coefficients[0:4]
        t = a*a + b*b + c*c
        if t == 0.0: raise ValueError
        t=np.sqrt(t)
        # normalize coef
        self.coeff = [a/t, b/t, c/t, d/t]

    def __repr__(self):
        return "Plane3D "+str(self.coeff)
    def __str__(self):
        return str(self.coeff)
        

    def distance_signed(self, p):
        """ returns the signed distance between a point and the plane.
        
        The sign is positive if the point p sits on the same side as the norm,
        and negative otherwise. Suppose p=[x, y, z], then the signed
        distance is:
        distance = (a*x + b*y + c*z + d) / sqrt(a*a + b*b + c*c)
        The plane-to-plane distance is to be implemented. 
        """
        a, b, c, d = self.coeff
        dist = a*p[0] + b*p[1] + c*p[2] + d
        return dist
    
    
    def distance(self, p):
        """ returns the distance between a point and the plane.
        
        Suppose p=[x, y, z], then the signed
        distance is
        distance = abs(a*x + b*y + c*z + d) / sqrt(a*a + b*b + c*c)
        The plane-plane distance is to be implemented. 
        """
        dist = self.distance_signed(p)
        if dist<0: 
            return -dist
        else: 
            return dist
    
    
    def as_list(self):
        """returns a list of the coefficient."""
        return self.coeff
        

    def is_parallel(self, another_plane):
        """ returns True if this plane is parallel to another, False otherwise.
        """
        t = np.cross(np.array(self.coeff[0:3]), 
                     np.array(another_plane.coeff[0:3]) )
        if t[0]*t[0] + t[1]*t[1] + t[2]*t[2] == 0.0:
            return True
        else: 
            return False
        

    def overlaps(self, another_plane):
        """ returns True if this plane is overlapping with another."""
        if self.coeff[0]==another_plane.coeff[0] and \
           self.coeff[1]==another_plane.coeff[1] and \
           self.coeff[2]==another_plane.coeff[2] and \
           self.coeff[3]==another_plane.coeff[3] :
           return True
        else:
            return False


    def set_distance(self, h):
        """ translate a plane so that the distance between origin is h.
        
        h: a number. The actual signed distance H = -h.
            If H>0 (that is h<0), the normal vector and the point are at the 
            same side of the plane. Otherwise, opposite sides.
        Note: since I want origin (that is, (0,0,0)) to be surrounded by 
        the plane, the signed distance between origin and the plane should 
        be negative. However, in most output files, the distance is a 
        positive value. So, I respect this convention. """
        #no need to reassure that a**2+b**2+c**2==1. They already are.
        #a, b, c = self.coeff[0:3]
        #self.coeff[3] = -h * np.sqrt(a*a + b*b + c*c)
        self.coeff[3] = -h


    def get_normal(self):
        """ return a copy of the plane's normal vector, as a tuple."""
        return (self.coeff[0], self.coeff[1], self.coeff[2])


    def intercept(self, p1, p2):
        """ returns the interception point among three planes.
        
        p1 and p2 are two other planes. If any two of the three planes are
        parallel, an empty array [] is returned.
        """
        if self.is_parallel(p1) or self.is_parallel(p2) or p1.is_parallel(p2) :
            return None
        
        A = np.array([self.coeff[0:3], p1.coeff[0:3], p2.coeff[0:3]])
        B = np.array([-self.coeff[3], -p1.coeff[3], -p2.coeff[3]])
        try :
            p = np.linalg.solve(A,B)
        except (np.linalg.LinAlgError) :
            return None
        return np.array(p)
    

    def sort_points(self, points_array):
        """ rearrange the points counter-clockwise around normal direction.
        
        This method does not change the order of the points.
        
        returns: [sorted_index]. """
        # rotate to align the plan perpendicular to the [001] direction.
        n_points = len(points_array)
        if n_points < 3 :
            return range(n_points)
                
        if self.coeff[0] != 0.0 or self.coeff[1] != 0.0 :
            plane_norm = self.get_normal()
            dir_z = np.array([0, 0, 1.0])
            axis_rotate = np.cross(plane_norm, dir_z)
            angle_rotate = np.arccos(np.inner(plane_norm, dir_z))
            # note plane_norm and dir_z are unit vectors.
            
            R = rotation_matrix(axis_rotate, angle_rotate)
            pa = np.array( (R *  np.matrix(points_array).T ).T )
        else :
            pa = np.array(points_array)
        
        # compute the center of the points
        center_points = np.array([0.0, 0, 0])
        for i in range(n_points) :
            center_points += pa[i]
        center_points /= n_points
        
        # move all points centered around the center_points
        pa -= center_points
        
        # compute the angles of each points with respect
        angles = np.zeros(n_points)
        for i in range(n_points) :
            x = pa[i][0]
            y = pa[i][1]
            if x==0 :
                if y>0 : angles[i] = np.pi/2
                else : angles[i] = np.pi * 1.5
            else :
                theta = np.arctan(y/x)
                if x<0 :angles[i] = theta + np.pi
                elif x>0 and y<0 : angles[i] = theta + np.pi*2
                else : angles[i] = theta
        
        # sort the angles
        ia_sorted = sorted(enumerate(angles), key=itemgetter(1))
        return [x[0] for x in ia_sorted]


    def area_convex(self, points_array, index_sorted=None):
        """ calculates the area bounded by a set of points on this plane.
        
        Points_array's shape is (n_points, 3). The points are assumed to be
        on this plane, so that no check is made about this assumption.
        
        returns: area.
        """
        # returns zero if there are less than 3 points
        n_points = len(points_array)
        if n_points < 3: return 0.0
              
        # rearrange the points counter-clockwise if necessary
        if index_sorted == None:
            ia_sorted = self.sort_points(points_array)
        else :
            ia_sorted = index_sorted
        
        # calculate the area of the bounded plane
        area = 0.0
        a1 = points_array[ ia_sorted[0] ]   # this point is fixed
        a2 = points_array[ ia_sorted[1] ]   # this point changes
        edge3 = vec_len(a1-a2)
        for i in range(2, n_points) :
            a3 = points_array[ ia_sorted[i] ]   # this point changes
            edge1 = vec_len(a2-a3)
            edge2 = vec_len(a3-a1)
            area += area_triangle(edge1, edge2, edge3)
            
            a2 = points_array[ ia_sorted[i] ]    # for the next triangle
            edge3 = edge2    # no need to calculate it again
         
        if np.isnan(area) :
            print "I found an NaN when calculating area. The result is likely to be undefined."  
        return area


def by_interception(x, y, z):
    """ generates a plane by the interceptions with the three coordinate axis.
    
    Limitations: the plane cannot pass origin otherwise the interception is
    (0, 0, 0); the plane cannot be parallel to one of the axis. The latter
    limitation can be lifted using a more universal method:
    by_inverse_interception().
    """
    if x==0.0 or y==0.0 or z==0.0: return None
    return Plane3d([1/x, 1/y, 1/z, -1.0])


def by_inverse_interception(x, y, z):
    """ generates a plane by the inverse interceptions.
    
    The actual interception is (1/x, 1/y, 1/z).
    Limitations: the plane cannot pass origin otherwise the interception is
    (0, 0, 0); when x=0, the plane is parallel to x-axis, and analogically 
    for y=0 or z=0. But, x, y, z cannot be zero at the same time.
    """
    if x==0.0 and y==0.0 and z==0.0: return None
    return Plane3d([x, y, z, -1.0])


def by_normal_point(v, p):
    """ Defines a plane by its normal and a point lying on the plane.

    The normal (argument v) cannot be zero."""
    if v[0]==0 and v[1]==0 and v[2]==0: 
        raise ValueError("The normal vector is zero.")
    t = -np.dot(v,p)   # t is the signed origin-to-plane distance
    # let the normal vector always point out from the origin
    if (t<0.0):
        return Plane3D([v[0], v[1], v[2], t])
    else:
        return Plane3D([-v[0], -v[1], -v[2], -t])

    
def by_3points(p1, p2, p3):
    """ Returns a plane passing through three points.

    The three points must be non-collinear, and any of them must not be (0,0,0),
    otherwise a ValueError occur."""
    #The following will be checked in function by_normal_point()
    #D = _det3x3([p1,p2,p3])
    #if D==0: raise ValueError("The three points are collinear.")
    v1 = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]]
    v2 = [p3[0]-p1[0], p3[1]-p1[1], p3[2]-p1[2]]
    v = np.cross(v1,v2)
    return by_normal_point(v, p1)
    

def by_2points_1vector(p1, p2, v):
    """ Returns a plane passing through two points and one vector lying on it.

    The two points must not be parallel to the vector.
    otherwise a ValueError occur."""
    v2 = [p2[0]-p1[0], p2[1]-p1[1], p2[2]-p1[2]]
    n = np.cross(v2, v)
    return by_normal_point(n, p1)


def by_1point_2vectors(p1, v1, v2):
    """ Returns a plane passing through one point and two vectors lying on it.

    The two vectors must not be linearly dependent,
    otherwise a ValueError occur."""
    v = np.cross(v1, v2)
    return by_normal_point(v, p1)


# testing and debugging
if __name__=="__main__" :
    a = [1.0, 1.0, 1.0, -1.0]
    p1 = Plane3D(a)
    print p1

    p2 = by_2points_1vector([0.0, 1.0, 1.0], [2.0, 0.0, 0.0], 
        [1.0, -1.0, 0.0])
    print p2

    p3 = by_1point_2vectors([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], 
        [0.0, 0.0, 1.0])
    print p3
